#include <QtGui>
#include "Spreadsheet.h"
#include "Matrix.h"
#include "Helper.h"
#include "FileIO.h"
#include <vector>
#include <string>
#define _USE_MATH_DEFINES // this makes sure M_PI will be defined
#include <math.h>

#include "Disturbance.h"

void CDisturbance::clear()
{
    setKay().CleanUp();
    setRho().CleanUp();
    setAa().CleanUp();
    setDisturbanceValues().CleanUp();
    setRunTime().clear(); 
    return;
}
void CDisturbance::precompute()
{
    precomputeTime(&setRunTime());
    return;
}
void CDisturbance::precomputeEachTrial()
{
    precomputeRho(&setRho());
    const int& nSines = numSines();
    const int& nDiffOrder = diffOrder();
    const CMatrix<double>& mAa = aa();
    const CMatrix<double>& mKay=kay();
    const vector<double>& vRunTime=runTime();
    const CMatrix<double>& mRho=rho();
    precomputeDisturbance(nSines,nDiffOrder,mAa,mKay,vRunTime,mRho,&setDisturbanceValues());
    return;
}

void CDisturbance::precomputeTime(vector<double>* pRunTime_)
{
    int nFrames			=  totalFrames();
    double dTimeStep	= 1./frameRate();
    (*pRunTime_).resize(nFrames);
    for(int t=0;t<nFrames;t++)
    {
        (*pRunTime_)[t]= t*dTimeStep;
    }
    return;
}
void CDisturbance::precomputeRho(CMatrix<double>* pmRho_)
{
    int nCol = numSines();
    (*pmRho_).SetSize(1,nCol);

    for (int c = 0; c < nCol; c++)
    {
        (*pmRho_)(c) = 2.0 * M_PI * Randrand()-M_PI;
    }
    return;
}

void CDisturbance::precomputeDisturbance(const int& nSines_, const int& nDiffOrder_, const CMatrix<double>& mAa_, const CMatrix<double>& mKay_, const vector<double>& vRunTime_, const CMatrix<double>& mRho_, CMatrix<double>* pmDisturbanceValues_)
{
    (*pmDisturbanceValues_).SetSize(vRunTime_.size(),1);

    setDeltaTee()=0;

    //for each disturbance, find the first crossing point with x-axis
    //reasons?
    unsigned int t; 
    for ( t = 0; t < vRunTime_.size(); t++)
    {
        (*pmDisturbanceValues_)(t) = sumOfSines(nSines_, nDiffOrder_, mAa_, mKay_, vRunTime_[t], mRho_);
        if ( t>0 && (*pmDisturbanceValues_)(t) * (*pmDisturbanceValues_)(t-1) < 0 )
        {
            setDeltaTee() = vRunTime_[t];
            break;
        }
    }
    
    double dTm1,dTc;
    dTm1 = vRunTime_[t-1]; dTc = vRunTime_[t];

    double dT,dTMin;
    double dSOS,dSOSMin;

    dSOSMin = 100;
    for (int i=0;i<1000;i++)
    {
        dT = dTm1 + (dTc - dTm1)/1000.*i;
        dSOS = sumOfSines(nSines_,nDiffOrder_,mAa_,mKay_,dT,mRho_);
        if(abs(dSOS)<abs(dSOSMin))
        {
            dSOSMin = dSOS;
            dTMin = dT;
        }
    }
    do
    {
        dT = dTm1 + (dTc - dTm1)/2.;
        dSOS = sumOfSines(nSines_,nDiffOrder_,mAa_,mKay_,dT,mRho_);
        if(dSOS*(*pmDisturbanceValues_)(t)<0)
        {
            dTm1 = dT;
            (*pmDisturbanceValues_)(t-1) = dSOS;
        }
        else if(dSOS*(*pmDisturbanceValues_)(t-1)<0)
        {
            dTc = dT;
            (*pmDisturbanceValues_)(t) = dSOS;
        }
    }
    while(abs(dSOS)>0.0000005);

    setDeltaTee() = dT;

    // use the crossing values as the starting points when pre-computing the disturbance.
    (*pmDisturbanceValues_).SetZeros(); // set our output back to zero
    for (unsigned int t = 0; t < vRunTime_.size(); t++)
    {
        (*pmDisturbanceValues_)(t) = disturbanceGain()*sumOfSines(nSines_, nDiffOrder_,  mAa_, mKay_, vRunTime_[t]+ deltaTee(), mRho_);
    }
    return;
}

double CDisturbance::sumOfSines(const int& nSines_, const int& nDiffOrder_,  const CMatrix<double>& mAa_, const CMatrix<double>& mKay_, const double& dRunTime_, const CMatrix<double>& mRho_)
{
    double dPreOmega = 2.0 * M_PI / trialLengthinSecs(); // 90 is second of each trial
    double dSumOfSines = .0;

    for (int c = nSines_-1; c >= 0 ; c--)
    {
        double dOmega = dPreOmega * mKay_(c);
        switch (nDiffOrder_) // nCurrDist_ is current disturbance
        {
        case 0: 
            dSumOfSines += mAa_(c) *                   sin(dOmega*dRunTime_ + mRho_(c));
            break;
        case 1:
            dSumOfSines += mAa_(c) * dOmega *          cos(dOmega*dRunTime_ + mRho_(c));
            break;
        case 2: 
            dSumOfSines += mAa_(c) * dOmega * dOmega * sin(dOmega*dRunTime_ + mRho_(c));
            break;
        case 3: 
            dSumOfSines += mAa_(c) * dOmega * dOmega * dOmega * cos(dOmega*dRunTime_ + mRho_(c));
            break;
        default:
            QMessageBox::warning(NULL,QString("Sum of Sine"),QString("Undefined order %1").arg(nDiffOrder_),QMessageBox::Yes); 
            break;
        }
    }
    return dSumOfSines;
}

void CDisturbance::load(const vector<CSpreadsheet*>& vSpreadsheet_,const string& strFileName_ )
{
    // load spectral file
    clear();
    // load spectral velocity data
    CSpreadsheet *pSpectral = NULL;
    for(unsigned int i=0;i<vSpreadsheet_.size();i++)
    {
        if(vSpreadsheet_[i]->currentFile().contains(strFileName_.c_str()))
            pSpectral = vSpreadsheet_[i];
    }
    if(!pSpectral)
    {
        QString sError("Spectral File : can not find any Spectral file.\n \"");
        sError += pSpectral->currentFile();
        sError += "\".\n";
        throw exception(sError.toStdString().c_str());
    }
    vector<double> vIn;
    string sIn;
    pSpectral->search("#Name:",1,&sIn);               setNameDisturbance() = sIn;
    pSpectral->search("#DistGain:",1,&vIn);           setDisturbanceGain() = vIn[0];
    pSpectral->search("#NumOfSines:",1,&vIn);         setNumSines() = vIn[0];
    pSpectral->search("#Trial Length in sec",1,&vIn); setTrialLengthinSecs() = vIn[0];
    pSpectral->search("#Extra Time in sec",1,&vIn);   setExtraSecs() = vIn[0];
    pSpectral->search("#Pre Run Time in sec",1,&vIn); setPreRunTimeinSecs() = vIn[0];
    pSpectral->search("#Frame Rate",1,&vIn);          SetFrameRate() = vIn[0];
    pSpectral->search("#Order",1,&vIn);               setDiffOrder() = vIn[0];
    setKay().SetSize(numSines(),1,0);
    setAa().SetSize(numSines(),1,0);
    CMatrix<double> mM;
    pSpectral->searchMatrix("#Disturbance 1 Kay Frequency",&mM);  setKay().Set(0,0,mM);
    pSpectral->searchMatrix("#Disturbance 1 Aa",&mM);             setAa().Set(0,0,mM);

    return ;
}


